# Modern Robotics Descriptions for all eight Interbotix Arms.
# Note that the end-effector is positioned at '/ee_arm_link'
# and that the Space frame is positioned at '/base_link'.

import numpy as np

class px100:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.09305, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.19305, 0.0, -0.035],
                      [0.0, -1.0, 0.0, 0.19305, 0.0, -0.135]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.248575],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.19305],
                  [0.0, 0.0, 0.0, 1.0]])

class px150:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.10391, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.25391, 0.0, -0.05],
                      [0.0, -1.0, 0.0, 0.25391, 0.0, -0.2],
                      [1.0, 0.0, 0.0, 0.0, 0.25391, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.358575],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.25391],
                  [0.0, 0.0, 0.0, 1.0]])

class rx150:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.10391, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.25391, 0.0, -0.05],
                      [0.0, -1.0, 0.0, 0.25391, 0.0, -0.2],
                      [1.0, 0.0, 0.0, 0.0, 0.25391, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.358575],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.25931],
                  [0.0, 0.0, 0.0, 1.0]])

class rx200:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.10391, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.30391, 0.0, -0.05],
                      [0.0, -1.0, 0.0, 0.30391, 0.0, -0.25],
                      [1.0, 0.0, 0.0, 0.0, 0.30391, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.408575],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.30391],
                  [0.0, 0.0, 0.0, 1.0]])

class vx250:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.12675, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.37675, 0.0, -0.06],
                      [0.0, -1.0, 0.0, 0.37675, 0.0, -0.31],
                      [1.0, 0.0, 0.0, 0.0, 0.37675, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.468575],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.37675],
                  [0.0, 0.0, 0.0, 1.0]])

class vx300:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.12675, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.42675, 0.0, -0.05955],
                      [0.0, -1.0, 0.0, 0.42675, 0.0, -0.35955],
                      [1.0, 0.0, 0.0, 0.0, 0.42675, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.536494],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.42675],
                  [0.0, 0.0, 0.0, 1.0]])

class vx300s:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.12675, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.42675, 0.0, -0.05955],
                      [1.0, 0.0, 0.0, 0.0, 0.42675, 0.0],
                      [0.0, -1.0, 0.0, 0.42675, 0.0, -0.35955],
                      [1.0, 0.0, 0.0, 0.0, 0.42675, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.536494],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.42675],
                  [0.0, 0.0, 0.0, 1.0]])

class wx200:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.11025, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.31025, 0.0, -0.05],
                      [0.0, -1.0, 0.0, 0.31025, 0.0, -0.25],
                      [1.0, 0.0, 0.0, 0.0, 0.31025, 0.0]]).T

    Blist = np.array([[0.0, 0.0, 1.0, 0.0, 0.358, 0.0],
                      [0.0, 1.0, 0.0, 0.2, 0.0, -0.358],
                      [0.0, -1.0, 0.0, 0.0, 0.0, 0.308],
                      [0.0, -1.0, 0.0, 0.0, 0.0, 0.108],
                      [1.0, 0.0, 0.0, 0.0 ,0.0, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.408575],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.31025],
                  [0.0, 0.0, 0.0, 1.0]])

class wx250:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.11025, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.36025, 0.0, -0.04975],
                      [0.0, -1.0, 0.0, 0.36025, 0.0, -0.29975],
                      [1.0, 0.0, 0.0, 0.0, 0.36025, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.458325],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.36025],
                  [0.0, 0.0, 0.0, 1.0]])

class wx250s:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.11025, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.36025, 0.0, -0.04975],
                      [1.0, 0.0, 0.0, 0.0, 0.36025, 0.0],
                      [0.0, -1.0, 0.0, 0.36025, 0.0, -0.29975],
                      [1.0, 0.0, 0.0, 0.0, 0.36025, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.458325],
                  [0.0, 1.0, 0.0, 0.0],
                  [0.0, 0.0, 1.0, 0.36025],
                  [0.0, 0.0, 0.0, 1.0]])
